#include "obstacle_detector/trajectory_predictor.h"

namespace obstacle_detector
{
TrajectoryPredictor::TrajectoryPredictor(bool debug):
debug_(debug)
{
	polynominal_values_.resize(polynominal_order_+1,301);
	get_polynominal_values();
}

void TrajectoryPredictor::get_predicted_trajectory(
	std::vector<Object> &objects,const std::vector<float> &roads_yaw)
{
	for (int i=0;i<objects.size();i++)
	{
		// printf ("objects %d history locs: %d\n",i,objects[i].history_locs.size());
		// int count=objects[i].history_locs.size();
		int count=objects[i].history_v.size();
		if (count==0)
		{
			continue;
		}
		Eigen::VectorXf loc_x(count+1);
		Eigen::VectorXf loc_y(count+1);
		Eigen::VectorXf predict_loc_x;
		Eigen::VectorXf predict_loc_y;
		float dis=0;
		Eigen::Vector2f center=objects[i].global_center;
		objects[i].history_locs.clear();
		objects[i].history_locs.resize(count+1);
		objects[i].history_locs[count]=center;
		loc_x(count)=center(0);
		loc_y(count)=center(1);
		for (int j=0;j<count;j++)  
		{
			Eigen::Vector2f v=objects[i].history_v[count-1-j];
			center=center-v;
			objects[i].history_locs[count-1-j]=center;
			loc_x[count-1-j]=center[0];
			loc_y[count-1-j]=center[1];
			// if (j>0)
			// {
			// 	dis+=((objects[i].history_locs[j]-objects[i].history_locs[j-1]).norm());
			// }
		}

		count=objects[i].history_locs.size();
		// printf ("v loc size: %d,%d\n",objects[i].history_v.size(),objects[i].history_locs.size());
		for (int j=1;j<count;j++)
		{
			dis+=sqrt(pow(loc_x(j)-loc_x(j-1),2)+pow(loc_y(j)-loc_y(j-1),2));
		}

		bool angle_valid=true;
		for (int j=1;j<count-1;j++)
		{
			Eigen::Vector2f vector1,vector2;
			vector1<<loc_x(j-1)-loc_x(j),loc_y(j-1)-loc_y(j);
			vector2<<loc_x(j+1)-loc_x(j),loc_y(j+1)-loc_y(j);
			float cos_theta=(vector1(0)*vector2(0)+vector1(1)*vector2(1))/(vector1.norm()*vector2.norm());
			float theta=acos(cos_theta);
			if (theta<(M_PI/2))
			{
				angle_valid=false;
			}
		}

		float final_dis=sqrt(pow(loc_x[0]-loc_x[count-1],2)+pow(loc_y[0]-loc_y[count-1],2));
		// printf ("final_loc %d(%d): %f,%f,%d\n",i,objects[i].label,dis,final_dis,angle_valid);
		// objects[i].predict_locs=Eigen::MatrixXf::Constant(2,predict_num_)
		Eigen::Vector2f lst_loc=objects[i].history_locs[count-1];
		objects[i].predict_locs.resize(2,1);
		objects[i].predict_locs.col(0)=lst_loc;
		// if (is_stop(objects[i].history_v))
		// {
		// 	continue;
		// }
		if (final_dis<0.7*dis || dis==0 || final_dis==0)
		{
			// printf ("trajectory check fail: %d,%f,%f\n",i,dis,final_dis);
			continue;
		}
		else if (!angle_valid && final_dis<0.8*dis)
		{
			// printf ("trajectory check fail: %d,%f,%f,%d\n",i,dis,final_dis,angle_valid);
			continue;
		}

		// std::cout<<loc_x<<std::endl;
		if (count<10)// || fabs(rho)>0.999)
		{
			curve_fitting(loc_x,loc_y,predict_loc_x,predict_loc_y,2,0,loc_x.rows()-1);
		}
		else
		{
			Eigen::VectorXf linear_predict_loc_x,linear_predict_loc_y;
			int start_index=loc_x.rows()-10;
			//先进行直线拟合 拟合出最后10个点的直线作为切线方向 并估计切线上的点
			curve_fitting(loc_x.block(start_index,0,10,1),loc_y.block(start_index,0,10,1),
				linear_predict_loc_x,linear_predict_loc_y,2,start_index,loc_x.rows()-1);
			//将部分切线上的点加入到历史路径点中进行估计预测 目的是为了让预测的估计不会偏离切线太远
			auto part_linear_x=linear_predict_loc_x.block(0,0,predict_num_-20,1);
			auto part_linear_y=linear_predict_loc_y.block(0,0,predict_num_-20,1);
			Eigen::VectorXf all_loc_x(loc_x.rows()+part_linear_x.rows());
			Eigen::VectorXf all_loc_y(loc_x.rows()+part_linear_x.rows());
			all_loc_x.block(0,0,loc_x.rows(),1)=loc_x;
			all_loc_x.block(loc_x.rows(),0,part_linear_x.rows(),1)=part_linear_x;
			all_loc_y.block(0,0,loc_x.rows(),1)=loc_y;
			all_loc_y.block(loc_x.rows(),0,part_linear_x.rows(),1)=part_linear_y;
			// printf ("all_loc size: %d,%d,%d\n",count,all_loc_x.size(),all_loc_y.size());
			curve_fitting(all_loc_x,all_loc_y,predict_loc_x,predict_loc_y,3,0,loc_x.rows()-1);
			// curve_fitting(loc_x,loc_y,predict_loc_x,predict_loc_y,4,0,loc_x.rows()-1);
		}
		// printf ("predict_loc size: %d,%d\n",predict_loc_x.rows(),predict_loc_x.cols());
		// std::cout<<predict_loc_x<<std::endl;
		int p_count=std::min(2*count,predict_num_);
		objects[i].predict_locs.resize(2,p_count);
		objects[i].predict_locs.block(0,0,1,p_count)=predict_loc_x.block(0,0,p_count,1).transpose();
		objects[i].predict_locs.block(1,0,1,p_count)=predict_loc_y.block(0,0,p_count,1).transpose();

		// printf ("center %d: %f,%f,%f,%f\n",i,objects[i].global_center[0],objects[i].global_center[1],
		// 	objects[i].predict_locs(0,0),objects[i].predict_locs(1,0));
		// printf ("road_id %d: %d\n",i,objects[i].road_id);
		if (objects[i].road_id>=0)
		{
			project_predict_locs(objects[i].global_center,
				roads_yaw[objects[i].road_id],objects[i].predict_locs);
		}
		// std::cout<<objects[i].predict_locs.transpose()<<std::endl;
		// std::cout<<loc_x.transpose()<<std::endl;
		// std::cout<<loc_y.transpose()<<std::endl;
		// std::cout<<"********************"<<std::endl;
	}
}


void TrajectoryPredictor::project_predict_locs(const Eigen::Vector2f &center,float yaw,
		Eigen::MatrixXf &predict_locs)
{
	Eigen::Vector2f v1;
	v1<<cos(yaw),sin(yaw);
	for (int i=0;i<predict_locs.cols();i++)
	{
		Eigen::Vector2f v2=predict_locs.col(i)-center;
		float length=v1.dot(v2);
		predict_locs.col(i)=center+length*v1;
	}
}

bool TrajectoryPredictor::is_stop(const std::vector<Eigen::Vector2f> &history_v)
{
	int max_check=std::min(10,int(history_v.size()));
	int count=0;
	for (int i=0;i<max_check;i++)
	{
		int index=history_v.size()-1-i;
		Eigen::Vector2f v=history_v[index];
		if (fabs(v(0))<0.00001 && fabs(v(1))<0.00001)
		{
			count++;
		}
	}
	if (count>=5)
	{
		return true;
	}
	return false;
}

void TrajectoryPredictor::curve_fitting(const Eigen::VectorXf &x, 
	const Eigen::VectorXf &y,Eigen::VectorXf &p_x,Eigen::VectorXf &p_y,
	int order,int start_index1, int start_index2)
{
	Eigen::MatrixXf polynominal_values=polynominal_values_.block(0,start_index1,order,x.rows());
	Eigen::MatrixXf A=polynominal_values*polynominal_values.transpose();
	Eigen::VectorXf b1=polynominal_values*x;
	Eigen::VectorXf b2=polynominal_values*y;
	Eigen::VectorXf coefficients1 = A.colPivHouseholderQr().solve(b1);
	Eigen::VectorXf coefficients2 = A.colPivHouseholderQr().solve(b2);
	// std::cout<<coefficients1<<std::endl;


	Eigen::MatrixXf predict_polynominal_values=polynominal_values_.block(0,start_index2,order,predict_num_);
	p_x=predict_polynominal_values.transpose()*coefficients1;
	p_y=predict_polynominal_values.transpose()*coefficients2;
}

void TrajectoryPredictor::get_polynominal_values()
{
	for (int i=0;i<301;i++)
	{
		float x=i*0.1;
		polynominal_values_(0,i)=1;
		for (int j=1;j<=polynominal_order_;j++)
		{
			polynominal_values_(j,i)=pow(x,j);
		}
		// std::cout<<polynominal_values_.col(i).transpose()<<std::endl;
	}
}


}